/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.grt192.macro;

import com.googlecode.gunncsnavigation.telemetry.TelemetryPorts;
import com.grt192.actuator.spot.GRTDemoLED;
import com.grt192.controller.spot.TelemetryServer;
import com.grt192.core.GRTObject;
import com.grt192.core.controller.Macro;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.event.component.spot.PulseSwitchListener;
import com.grt192.mechanism.spot.MaxbotixSoftConstantLoop;
import com.grt192.mechanism.spot.SLight;
import com.grt192.rsh.Commandable;
import com.grt192.rsh.RSHServer;
import com.grt192.sensor.spot.GRTPulseSwitch;
import com.grt192.sensor.spot.GRTSonar;
import com.grt192.utils.Util;
import com.sun.spot.resources.transducers.IIOPin;
import com.sun.spot.sensorboard.EDemoBoard;

/**
 *
 * @author ajc
 */
public class SonarSpot extends GRTObject implements Macro,
        TelemetryPorts,
        SensorChangeListener,
        PulseSwitchListener,
        Commandable {

    public SonarSpot() {
        setPrinting(true);
        RSHServer.getInstance().addListener(this);
        RSHServer.getInstance().setPrinting(true);
    }

    public int execute() {
        initLights();
        startSensors();
        startListening();
        while (true) {
            Util.sleep(1000);
        }
    }
    private SLight[] lights = SLight.get();
    private int[] sonarLightIDs = new int[]{2, 3, 4};
    private int txLightID = 5;

    public void initLights() {
        //sonars
        for (int i = 0; i < sonarLightIDs.length; i++) {
            lights[sonarLightIDs[i]].rawColor(GRTDemoLED.Color.ORANGE);
            lights[sonarLightIDs[i]].blinkToBlack();
        }
        //tx pin
        lights[txLightID].rawColor(GRTDemoLED.Color.WHITE);
    }
    private static final int NUM_SONARS = 3;
    private static final int RX_PIN = 4;
    private static final int TX_PIN = 0;
    GRTSonar[] sonars;
    IIOPin rx;
    GRTPulseSwitch tx;
    MaxbotixSoftConstantLoop mscl;
    TelemetryServer[] servers;

    public void startSensors() {
        log("Starting sonars...");
        sonars = new GRTSonar[NUM_SONARS];
        for (int i = 0; i < sonars.length; i++) {
            sonars[i] = GRTSonar.fromAnalog(i, 50, "Sonar" + i);
//            sonars[i].setChangeThreshold(txLightID);
            sonars[i].setChangeThreshold(1);
            sonars[i].start();
            log("\t" + sonars[i].getID() + "started");
        }
        log("Setting rx " + RX_PIN + " low...");
        rx = EDemoBoard.getInstance().getIOPins()[RX_PIN];
        rx.setAsOutput(true);
        rx.setLow();

        log("Starting pulseswitch " + TX_PIN + " ...");
        tx = new GRTPulseSwitch(TX_PIN, 5, "d" + TX_PIN);
        tx.addPulseSwitchListener(this);
        tx.start();

        log("Starting MaxbotixDaisyChain...");
        mscl = new MaxbotixSoftConstantLoop(sonars, rx, tx);
        mscl.startRanging();

        log("Starting Telemetry servers...");
        servers = new TelemetryServer[NUM_SONARS];
        for (int i = 0; i < servers.length; i++) {
            servers[i] = new TelemetryServer(sonars[i], SONAR_PORTS[i]);
            servers[i].start();
            log("\t new TelemetryServer hosting " + sonars[i].getID() + " on port " + SONAR_PORTS[i]);
        }

    }

    public void startListening() {
        log("Listening to sonars...");
        for (int i = 0; i < sonars.length; i++) {
            sonars[i].addSensorChangeListener(this);
            log("\t listening to " + sonars[i].getID());
        }
    }

    public void sensorStateChanged(SensorEvent e, String key) {
        for (int i = 0; i < sonars.length; i++) {
            if (sonars[i] == e.getSource()) {
                lights[sonarLightIDs[i]].blinkToBlack();
                break;
            }
        }
        log("distance: " + e.getSource().getID() + ": " + e.getData(key));
    }

    public void pulseRead(GRTPulseSwitch s, int usec) {
        lights[txLightID].blinkToBlack();
        log("Pulse read for " + usec + " usecs");
    }

    public void command(String[] args) {
        log("SonarSpot commanded: " +Util.arraytoString(args));
//        mscl.star
    }
}
